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ABSTRACT 

A study is performed to estimate various states of a vehicle 
launched from an idealized spherical, airless, nonrotating earth. A 
tracking radar measures slant range and elevation of the vehicle and 
yields an output which is corrupted by additive Gaussian noise. In 
addition, the vehicle is disturbed by a random specific force during 
its boost phase. Estimation of the vehicle states is done by two dif¬ 
ferent Kalman filter mechanizations which are linearized applications 
of the optimal theory. A Monte Carlo computer simulation is used to 
compare the performance of the two filter mechanizations. 
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1. Introduction 


A large class of estimation problems is concerned with finding 
an optimal estimate of some quantity (an unknown parameter, a random 
variable, or a random signal) when a linear function of this quantity, 
corrupted by an additive noise, is available for generating the estimate. 
However, the class of estimation problems most often encountered are 
those in which the unknown quantity is describable by equations which 
are not linear functions. Then, the theory of Kalman [1] and Kalman 
and Bucy [2], developed to indicate optimal state estimates of linear 
systems, no longer yields an optimal estimate. Extensions or new 
theory work to include a large class of truely nonlinear systems gen¬ 
erally has not been successfully developed and is the topic of current 
research. A partial solution, allowing application of their theory in 
many instances to practical nonlinear systems, is based on work by 
Kushner f3], Cox [4], Bucy [5], and others. 

The fundamental assumption [6] is that a nominal solution of the 
system ' s nonlineardifferential equations must exist. This solution 
must provide a "good" approximation to the actual behavior of the system. 
It is "good" if the difference between the nominal and actual solution 
can be described by a set of linear differential equations (sometimes 
called linear perturbation equations). In practice the nominal equations 
or nominal trajectory may not be available a priori . For example, the 
navigation of a terrestrial vehicle may not be predetermined in the 
sense that the trajectory of a ballistic missile or space booster is 
predetermined. Without a nominal trajectory, the linear perturbation 
equations may be obtained as the difference between the actual solution 
and the current estimate of the nominal trajectory. This process, called 
relinearization by Bryson and Ho [7], may prove to be even more accurate 
in estimating the state variable than when linearizing about the nominal. 


2. Purpose 

It is the purpose of this study to evaluate two mechanizations 
of a pseudo nonlinear Kalman-Bucy filter. Performance will be evaluated 
of a filter that estimates the states of a nonlinear system which has 
been linearized about the nominal, called a linearized Kalman filter [8], 
versus one that has been re linearized about the current estimate, called 
an extended Kalman filter [8], 


3. Problem 

The problem is to estimate various parameters for a vehicle 
launched from an earth assumed to be spherical, airless, and nonrotating. 
The launch point and trajectory of the vehicle are coplanar with a tracking 
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station located downrange from the launch site. The tracking radar 
measures slant range and elevation of the vehicle and yields an output 
which is corrputed by noise. The noisy output is processed to give 
estimates of the vehicle's states which describe its trajectory. For 
this study the trajectory is assumed simply to be generated by a gravity 
turn. After launch the rocket is disturbed in the cross track direction 
by a random specific force which for realism could be considered a 
gusting wind disturbance. 

4. Model of the System 

All of the equations for the launch problem can be inferred 
from the geometry presented in Figure 1. 



Figure 1. Geometry of the Problem 





The following symbols are applied in Figure 1: 

R A earth's radius 
e 

L ^ elevation angle between radar site and vehicle 

9 £ reference angle between launch site and vehicle 

9 A reference angle between launch site and radar site 
K 

h ^ altitude above the earth's surface 

S ^ slant range distance from radar site to vehicle 

~g £ earth's gravity vector with magnitude at zero altitude 

v ^ velocity of the vehicle with respect to an inertial basis 

—* a 

F n s disturbing force orthogonal to v 

T & vehicles thrust vector with respect to an inertial basis 

y A flight path angle between velocity vector and a vehicle fixed 
reference line 

d A distance of the vehicle downrange from the launch site 

r A h + R . 

e 

The following state variable equations defining the vehicle's 
trajectory are obtainable from the given geometry of Figure 1: 

h = v sin 7 , ( 1 ) 


d = 


R g v cos 7 
h + R 


( 2 ) 


v = 


L_sin_L + g /T\-I- 

+ *.)* 1 - (f) r- 

\ o/ sp 


(3) 


. d k cos 7 . f n 

7 " R e ‘ v(h + R e ) 2 V 


(4) 
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where 


k ft gR g 

t = time of flight measured in seconds from the launch 

I ^ specific impulse of the rocket fuel 

w A initial weight of the total vehicle, 
o ~ 


The slant range and elevation equations as a function of the states 
are obtained from simple trigonometric identities. From the Law of 
Cosines r 

S 2 = r 2 + R g 2 ■ 2R e r cos (e R - e) , (5) 

so that 


S = 



+ R - 2 R r cos 


K ■ e ) 


( 6 ) 


From the Law of Sines: 


Sin ( 0 R " 9 ) sin(| + L) sin (rt - (§ + l) - (0 R - (?)) 


(7) 


Expanding the middle term of Equation (7), 


sin 


(!- 0 - 


sin ~ cos L + cos sin L 


= cos L 


( 8 ) 


Thus, 


cos L sin ^0 R - ej 1 


(9) 
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and 


L = arc cos 



r sin 


( e R~ 6 ) 


+ R e ■ 2 R e r cos ■ 9) 


( 10 ) 


Alternatively, if the right equality of Equation (7) is used the eleva¬ 
tion angle is given by 


L = arc tan 


cos(g R - 6) - T 


sin 


(*R - 9 ) 


( 11 ) 


5. Approximately Nonlinear Kalman-Bucy Filter Mechanizations 

The filter equations developed by Kalman and Bucy were derived 
under the assumption that the system disturbances and the measurement 
errors were random variables described by Gaussian statistics, and that 
the plant was describable by linear equations. The resulting filter 
then gave the optimal estimates of the states. 

The filter equations in this study are modeled as a continuous 
nonlinear system. That is, unlike a discrete model, the transition of 
each*state from one increment of time to another is considered to be 
a smooth continuous process. Furthermore, the observations measured 
with the radar tracking system are also continuous. Thus the system 
nonlinear dynamics are given by 


x = f[x(t), w(t), t] 

where 

x ~ n-vector of states 
w ^ m-vector of process noise 
t = time. 


The observation equation is also nonlinear and given by 


z(t) = h[x(t), t] + v(t) 


( 12 ) 


(13) 
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where 


z ^ p-vector of observations 
v = k-vector of measurement noise. 

The a priori statistics are given by zero means; i.e.. 



Efw(t)} = 0 

(14) 


E{v(t)) = 0 

(15) 

where E{*) is used to denote the expected value of a quantity 
noise covariance matrices are 

. The 


E jw(t) , w T ( t )} = QS (t - t) 

(16) 


E jv(t) , v T (,)| = R5(t - t) , 

(17) 


E jw(t), v (t ) j = 0 , 

(18) 

where 



6 (t 

- t) ^ Dirac delta function. 


To estimate the states a suboptimal nonlinear filter is 
estimates their deviation from the nominal trajectory or the 
estimated trajectory. The nominal trajectory is given by the 

used which 
current 
nonlinear 


state equations with the white process noise ignored, i.e., 


*»■ f [V t)> *] 


(19) 


where 


x A n-vector of nominal states. 

N ~ 

The actual trajectory is the trajectory represented by the launch and 
boost itself. In this study it is simulated by Equation (12) which 
contains the process noise as would be expected in a realistic situation. 
To linearize about the nominal state, define a small error as the 
difference between the nominal state and the actual state 


6x(t) = x(t) = x(t) - x N (t) 


( 20 ) 
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It is apparent that 


6x(t) = x(t) - * N (t) 


( 21 ) 


Putting Equation (21) into Equation (12) yields 


x(t) = x N (t) + Bx(t) = f[x N (t) +&x(t), w(t) +&w(t), t J . (22) 
Expanding the right side of Equation (22) via a Taylor series, 


Sffx^t), w(t), tl 

:Jx N (t) +&x(t), w(t) +5w(t), tj = fj^x N (t), w(t), tj +—*>—d x"(t) - A 


&x(t) 

X N 


^f|^x N (t), w(t), tj 

+ N /. \ ^ 


Sw(t) 


6w(t) + 0 

W N 


2 + 


(23) 


where 


df[x N (t), w(t), t] 


d* N <t) 


^ evaluation of the partial derivative at 
the nominal state 


N 


c)f[x N (t), w(t), t] 


c)w(t) 


^ evaluation of the partial derivative about 
the conditional mean 


w 


N 


2 + 

0 ^ partial derivative terms of order two 

and higher. 

Thus a differential equation that gives the deviation between the 
actual state and the nominal state to first order is 


dfQc^t), w(t), tjj 3f[x N (t), w(t), t] 

“f' \ \ 


5x = 


dx N (t) 


&x(t) 

X N 


^w(t) 


(state deviation linearized about the nominal) 


5w(t) . (24) 


*N 
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In a manner which uses Che Taylor series expansion as above, a 
differential equation can be formulated that describes the state evalu¬ 
ated about the estimate. By defining the error as the difference 


x(t) A x(t) = x(t) - x(t) 


(25) 


so that 


x(t) = x(t) - x(t) 


(26) 


and proceeding as before, the result is 


. df[x(t), w(t), t] 

x(t) 

. df[x(t), w(t), t] 

w(t) 

x - a£( t ) 

dw(t) 


A 

X 


A 

w 

(state linearized 

about the estimate) 



where the partial derivative for the noise is evaluated about the 
expected value. This is the conditional mean, as before. 

The observations given by Equation (13) are also a system of 
nonlinear equations which may be linearized by expansion of a Taylor 
series and evaluated about the nominal or estimated trajectory. Define 
the observation deviation as 


Sz(t) = z(t) - z N (t) 


(28) 


The observations contain only additive noise so that when the Taylor 
series expansion is performed, as was done previously, the results are 


5z(t) = 


dh[x N (t), t] 

d* N (t) 


5x N (t) + v(t) 
X N 


(observation deviation evaluated about the nominal) 


(29) 
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z(t) = 


2 h l s JLtL 


£*<t) 



x(t) + v(t) 

A 

X 


(observation evaluated about the estimate) 


Before expressing the algorithms for the suboptimal filter the 
following definitions will be useful: 


F„<t) & 


df^x^t), w(t), tj 


Sx N (t) 


N 


c N (t) s 


A w ( fc >> fc ] 


5w(t) 


w 


N 


* dhfx„(t), til 








Using the definitions previously described, the filter equations that 
describe the state deviations for the two mechanizations follow in 
Tables I and II. Derivations of these equations can be found in 
references [1], [2], and [6] through [9]. 


Table I. Linearized Kalman Filter Equations 


Filter state deviation 

• 

5x(t) = Fjj(t) &x(t) + K(t)[&z(t) - H N (t)5^(t)] 

Suboptimal gain 

K(t) =T(t) B^Ct) R _1 (t) 

Error equation 

5x(t) = &x(t) - &x(t) 

Error covariance 
matrix 

Z(t) = E|gx(t), Bx T (t)j 

Error covariance 

differential equation 

Z(t) = F N (t)Z(t) +Z(t)F N T (t) 

-Z(t)H N T (t)R" 1 (t)H N (t)Z(t) 

+ c N (t) Q(t) G N T (t) 

Best complete 

linearized state 
estimate 

x(t) = x N (t) + Bx(t) 

















Table 11. Extended Kalman Filter Equations 


Filter State 
equation 

• 

x(t) = f[x(t), t] + K(t) [z(t) - h(x(t), t)] 

Suboptimal gain 

K(t) = Z(t) HgV) R -1 (t) 

Error equation 

x(t) = x(t) - x(t) 

Error covariance 
matrix 

Z( t) = e| x(t), x T (t)J 

Error covariance 
differential 
equation 

Z(t) = F E (t)Z(t) +Z(t) F E T (t) 

- Z(t) Hg T (t) R - 1 (t) Hg(t)Z(t) 

+ G E (t) Q(t) G E T (t) 


6. Development of the Filter Models 

a. The Explicit Linearized Equations 

The algorithms summarized in Tables I and II require 
the evaluation of partial derivatives about the nominal and estimated 
trajectory. Equation (31) is a matrix operation which is derivable 
from the additional information which follows. Because 


f 1 [x(t), w(t), t] 


f[x(t), w(t), t] = 


(37) 


f n [x(t), w(t), t] 
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X(t) 


x L (t) 


x n ( t ) 


(38) 


then. 


F„(t) - 


3f|x N (t), w(t), tj 


dx N (t) 


N 


5x n 


• * 


Sx t 


!£l 

Sx_ 


• • 


df 

n 

sr 

n 


N 


, (39) 


where the arguments have been dropped for simplicity. It is obvious, 
but worth repeating, that 


f 1 [x(t), w(t), t] - h «= v sin 7 , 


R v cos 7 

• o ' 


f 2 (x(t) , w(t) , t] * d - h + R 


(40) 

(41) 


f 3 [x ( 0 , «(t). t| + . (M) 

V V sp 


f,[x(t), w(t), t] - r - r- - -T— 8 \ + -7T 


e v 


(" + \Y 


(43) 
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and that 


x t (t) g h(t) 

(44) 

x 2 (t) £ d(t) 

(45) 

x 3 (t) ^ v(t) , 

(46) 

x 4 (t) A r( t) 

(47) 


By performing the differentiation described by Equation (39), the 
following four by four array is obtained: 


f n * 


- R v com y 

T h * 


Zk »ln r 




3 


sin y 


R t CO* 7 

h + R 


[-v 2 (h4»,),2R]co*7 

/. _ \3 . 21 . . « \2 


CO* 7 


v( h + *.) 


*X h+ 


v co* y 


- R e v *ln y 


h + R 


- R 5g?-r.. 

( h + v) 


- v^h + »J 2 + k .in 7 
v(h\ R ,) 2 


. (48) 


By an analogous operation described by Equation (32), the following 

array for G„(t) is completely filled with zeros except for the element 
N 

in the fourth row an'; fourth column: 


G N (t) 



(49) 
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For the observation equations, the functions are 



H = / 


2 2 
r + R - 2R 


cm ( 9 r - 9 ) 


and 


h 2 [x B (t>, t] = 


L = 


arctan 




(50) 


(51) 


Performing the operation described by Equation (33) results in the 
following two by four array: 



dh x 

5hj 

ah t at^ 




dXj^ 

9x 2 

ax^ ax 4 



v c > - 

dh 2 

ah 2 

Sh^ ah 2 

* 

(52) 



b* 2 

Sxj Sx 4 

x 






n 


The arguments on the partials have been left out for simplicity and 



9x^ 

oh^ dh 2 

ah 

“ ^7 = 0 

4 

> 

(53) 

also. 






dhj 

dxj 

R 

. _e 

s 

( l + t‘ 

cos ( 9 R - 9 ) 

) • 

(54) 

dh^ 
dx 2 = 

*(‘♦4 

) s ln (a R - e) 

» 

(55) 
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and 


cos 


(Sr ~ j ) 


dhj 

Sx, 


1 + R 



(56) 


(57) 


The arrays for the expressions given by Equations (34), (35), and (36) 
are exactly the same with the important restriction that they are to be 
evaluated about the current estimate and not about the nominal. 


b. Computer Synthesis of Suboptimal Estimators 

The synthesis of a computer algorithm for the computations 
required to estimate the four states of the idealized rocket launch can 
be observed in Figures 2 through 5. These are similar to results derived 
by Lange [10], Figure 2 is a block diagram of the overall program with 
the linearization done about a nominal trajectory. Figure 3, in more 
detail, describes the linearized filter process. Figure 4, in more 
detail, describes the actual and nominal trajectories. 

The synthesis for the extended filter is essentially the same but 
with the major exception that the estimated state equations replace the 
nominal state equations in Figures 2 and 4. The estimated equations 
are formed by adding the observation differences multiplied by the opti¬ 
mal gain. Figure 5 shows this process. 

c. Discussion of the Model and Problem 


The problem, mentioned earlier, is to eitimate four 
parameters of an idealized vehicle's trajectory launched from a spherical, 
airless,'nonrotating earth. The trajectory parameters are altitude (h), 
downrange distance traveled as measured from the launch site (d), 
velocity (v), and flight path angle (y). The launch site and trajectory 
of the vehicle are in a plane which contains a radar tracking station 
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SEE FIGURE 4 FOR DETAIL 




























ACTUAL 



Detail of Nominal and Actual Trajectory Development 

















Figure 5. Formation of Estimated Nonlinear Equations 


located 40 nautical miles from the launch site. The simulation is run 
with the noise (w), which is considered a disturbing wind, at a value 
of zero until 15 seconds has elapsed. This can be envisioned as a 
case where the surface winds are negligible for altitudes up to about 
2500 feet. 

The trajectory is planned to achieve an altitude of about 80 miles 
with a flight path angle suitable for injection into a circular orbit. 
Thus, at the time of 220 seconds, the ideal case would be to have y 
equal to zero. 

In the runs where the Extended Kalman Filter is used, the initial 
estimated equations are assumed to be the same as the actual equations 
but without noise. After 10 seconds the estimated equations are 
generated more accurately by employing the observation modified by the 
optimal gains as summarized in Table II. 

Also, the elevation angle (L) measured by the tracking radar is 
not computed for approximately 10 seconds. This is to allow the vehicle, 
which initially appears below the horizon to the radar tracker, to 
become observable. The impact of this modification is to prevent the 
measurement noise covariance matrix (R) from being computed while it 
is zero. This keeps the computer from reaching an exponential overflow, 

i.e., (R) ^ becomes infinite and the simulation terminates prematurely. 

In the initialization of the problem, several constants, initial 
conditions, and standard deviations of measurement error had to be 
chosen. Tables III and IV are compilation of those values. 








Table III. State Initial Conditions and Constants 


States 

Constants 

v ■ 100 ft/sec 

T/w « 1.3 

o 

o 

d - 0 ft 

R - 20.89 x 10 6 ft 

o 

e 

h - 0 ft 
o 

I * 300 sec 
sp 

= 89.66 deg 

g = 32.17 ft/sec 2 


d m 40 n mi 


NOTE: Because this analysis assures Gaussian statistics, the standard 
deviation about a zero mean defines the probability distribution func¬ 
tion precisely. The 1-cr values for the driving noise and measurement 
noise are given in Table IV. 


Table IV. Standard Deviation of Noise 


Driving Noise 

Measurement Noise 

o v * 0 ft/sec 

o » 100 ft 
s 

a d = 10 ft 

a L = 0.01 rad 

a h = 100 ft 


a = 0.001 deg 
y 



Finally, the covariance matrices (£), (R), and (Q) are initialized 
with the appropriate element given by the square of the standard devia¬ 
tions of Table IV. The initial array for the error covariance matrix is 


'io 4 

0 

0 

I 0 

0 

CM 

o 

r-4 

0 

1 

I 0 

0 

0 

0 

1 

1 0 

_0 

0 

0 

! 10 6 /(57.7) 2 _ 


(58) 
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the initial array for the measurement noise covariance matrix is 



( 59 ) 


and the initial array for the inverse of the process noise covariance 
matrix is 



(60) 


7. Results and Conclusions 

The performance of the two mechanizations used to estimate 
the trajectory states are observable in Figures 6 through 18. The values 
of plus and minus one standard deviation from the indicated covariance 
matrix are plotted in Figures 6 through 13. That is, each figure has the 
square root of its corresponding diagonal element of the covariance 
matrix plotted for a positive and negative 1-cr value. Also, each figure 
shows the error of the filter in estimating that state. For the Extended 
Kalman Filter Mechanization (Figures 6 through 9), the error is given 
as the difference between the actual state and the estimate of the 
actual state. For the Linearized Kalman Filter Mechanization (Figures 
10 through 13), the filter error is given as the difference between the 
actual state and the best estimate of the actual state which in turn 
has been differenced about a nominal, i.e.. 


X X actual X actual ~ X actual ^ X nominal J ' 

It is interesting to note that the results of both mechanizations indi¬ 
cate proper performance of the filter. This is concluded from the 
figures by observing that the irregular or "noise-like" trace is indeed 
within the ±1 a curves about 63 percent of the time as the linearized 
theory suggests. 
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Figure 6. Performance of the Extended Kalman Filter 
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Figure 7. Performance of the Linearized Kalman Filter 
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TIME M 

Figure 10. Performance of the Extended Kalman Filter 
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Figure 11. Performance of the Linearized Kalman Filter 


























Or an overlay depicting the extended filter variance versus the 
linearized filter variance, there is virtually no discernible difference 
between the two for any of the states estimated. However, Figures 14 
through 18 show a graphic view of the filter error in estimating the 
states. The velocity (v), distance downrange (d), and altitude (h) are 
estimated with less error with the Extended Kalman Filter Mechanization. 
The same conclusion holds true for the flight path angle ( 7 ), though 
not as graphically evident as for the first three states. 

At this point one is inclined to conclude that the Extended Kalman 
Filter (known also as the Relinearized Kalman Filter) is superior to 
the Linearized Kalman Filter in estimating states defined by nonlinear 
equations. However, in reporting on a larger sample of results by 
others, Jazwinski [9] notes that such results cannot be assessed a priori . 
That is, though better more often than not, the Extend Filter can some¬ 
times be worse and may even diverge (Kushner [3]). 

The future effort in the application of these mechanizations to 
nonlinear filtering problems is probably best directed to actual simu¬ 
lations as was done here. More experience with the approximate nonlinear 
filters is desirable because the insight gained in this type of work 
may lead to other useful approximation techniques. 


8. Future Applications 

This report describes the first phase of a study directed 
toward application in the Army's Cannon Launched Guided Projectile (CLGP). 
It is well known that the Proportional Navigation and Guidance Law used 
in the CLGP, though highly accurate for its mission. Is susceptible to 
the effects of the gravity field force. The result is that the pro¬ 
jectile's flight trajectory has a tendency to "droop" or fall short of 
the intended target at low quadrant elevation firings. One way to 
minimize this effect is to roll stabilize the projectile and, knowing 
the direction of the vertical, bias the autopilot to, in essence, aim 
high and counteract gravity. 

A common method for roll stabilization is to measure roll rate with 
a gyroscope sensor and null the vehicle's roll rate to some small level. 
In the CLGP environment, however, the gyroscopic sensor is subjected to 
extremely high acceleration forces at the launching. Thus, the question 
may be asked if an alternative method of measuring and nulling roll rate 
is available. The answer leads into the program for which this report 
was initiated. The program is to determine the feasibility of esti¬ 
mating roll rate of the CLGP vehicle without the requirement of a 
specific additional roll rate sensor. The solution, if it is feasible, 
would make use of other on-board sensors, such as the laser seeker, to 
estimate the required state. Implementation of this scheme is through 
formulation of a Kalman-Bucy filter which, in the linear theory, opti¬ 
mally yields the best estimate in a minimum variance sense. 
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Because the CLGP equations of motion are not linear, the problem 
is complicated by the choice of a linearization scheme. This report 
addresses that problem, in particular, for a simplified and idealized 
system of equations. However the results are directly applicable to the 
future work described above. 



Figure 14. Comparison of Filter Errors 
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Figure 16. Comparison of Filter Errors 
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